import serial
import serial.tools.list_ports
import struct
import time

SPD_LIMIT = 250.
ROTATE_LIMIT = 20.
X_POS_LIM = 200
Y_POS_LIM = 200


class Magnet_Plate_Robot():
    def __init__(self, COM_num=None, lock=None):
        self.lock = lock
        self.ser = serial.Serial()
        self.ser.baudrate = 115200
        if COM_num is not None:
            self.ser.port = COM_num
        else:
            self.ask_for_port()
        self.ser.timeout = 5
        self.spd_now = 0
        self.ser.open()
        self.stop()
        self.position_init()
    
    def ask_for_port(self):
        options = serial.tools.list_ports.comports()
        for k, i in enumerate(options):
            print(f"{k}: {i.description}")
        if options:
            port_num = int(input("Choose a port: "))
            self.ser.port = options[port_num].device

    def get_position(self):
        if self.lock is not None:
            self.lock.acquire()
        self.ser.read_all()
        msg = b"?\r\n"
        self.ser.write(msg)
        a = self.ser.read()
        buffer = None
        for _ in range(1024):
            b = self.ser.read()
            if a == b":" and b == b":":
                buffer = self.ser.read(16)
                break
            else:
                a = b
        if self.lock is not None:
            self.lock.release()
        if buffer is not None:
            x, y = struct.unpack("2q", buffer)
            x = x / 100000 * 1.875
            y = y / 100000 * 1.875
        else:
            x, y = None, None
        #print(x, y)
        return (x, y)

    def move(self, vec, spd):
        """用来输入相对坐标，单位为mm，速度单位为mm/s 提示1.875mm/s为100脉冲"""
        x ,y = vec
        spd = SPD_LIMIT if spd > SPD_LIMIT else spd
        # 防呆检测
        if (spd < 1e-1) or (abs(x)+abs(y) < 1e-3):
            # print(f"no move: {spd}, {x}, {y}")
            return
        x = X_POS_LIM if x > X_POS_LIM else x
        x = -X_POS_LIM if x < -X_POS_LIM else x
        y = Y_POS_LIM if y > Y_POS_LIM else y
        y = -Y_POS_LIM if y < -Y_POS_LIM else y


        self.spd_now = spd
        move_time = ((x **2 + y **2) **0.5) / self.spd_now
        # print(move_time)
        # spd单位mm/s
        spdx = x / move_time
        spdy = y / move_time
        
        freqx = (spdx/3) * 160
        freqy = (spdy/3) * 160

        if self.ser.isOpen():
            try:
                moto_0_msg = f":0 {int(freqx)}\r\n".encode()
                moto_1_msg = f":1 {int(freqy)}\r\n".encode()
                self.ser.write(moto_0_msg)
                self.ser.write(moto_1_msg)
                time.sleep(move_time)
                if self.lock is not None:
                    self.lock.acquire()
                self.ser.write(b":1 0\r\n")
                self.ser.write(b":0 0\r\n")
                if self.lock is not None:
                    self.lock.release()
                self.position = self.get_position()
            except BaseException as e:
                print("invalid msg" + str(e))
            # print(self.position)

    def position_init(self):
        """复位函数，回到原点"""
        if self.ser.isOpen():
            self.set_speed(-120, -120)
            time.sleep(3)
            if self.lock is not None:
                self.lock.acquire()
            msg = b"r\r\n"
            self.ser.write(msg)
            if self.lock is not None:
                self.lock.release()
            self.position = (0,0)
    
    def move_to(self,vec,spd):
        """绝对坐标,总的地图尺寸为160mm*160mm"""
        x1, y1 = vec
        x0, y0 = self.get_position()
        dx = x1 - x0
        dy = y1 - y0
        if self.ser.isOpen():
            self.move((dx,dy), spd)

    def set_rotation(self,rps):
        """输入电机转速，单位为 圈/秒"""
        rps = ROTATE_LIMIT if rps > ROTATE_LIMIT else rps
        rps = -ROTATE_LIMIT if rps < -ROTATE_LIMIT else rps
        spd_target = int(rps * 1600)
        while self.spd_now != spd_target:
            if self.spd_now < spd_target:
                self.spd_now += 1600
                self.spd_now = spd_target if self.spd_now > spd_target else self.spd_now
            if self.spd_now > spd_target:
                self.spd_now -= 1600
                self.spd_now = spd_target if self.spd_now < spd_target else self.spd_now
            if self.lock is not None:
                self.lock.acquire()
            self.ser.write(f":2 {self.spd_now}\r\n".encode())
            if self.lock is not None:
                self.lock.release()
            time.sleep(0.2)
    
    def set_speed(self, vx, vy):
        if abs(vx) + abs(vy) > 1e-2:
            freqx = (vx/3) * 160
            freqy = (vy/3) * 160
            moto_0_msg = f":0 {int(freqx)}\r\n".encode()
            moto_1_msg = f":1 {int(freqy)}\r\n".encode()
            if self.lock is not None:
                self.lock.acquire()
            self.ser.write(moto_0_msg)
            self.ser.write(moto_1_msg)
            if self.lock is not None:
                self.lock.release()
    

    def stop(self):
        self.ser.write(b"!\r\n")
